#include "ServoManager.h"


CServoManager::CServoManager()
{
	m_servoCnt = 0;
	for (int i = 0;i < SERVO_MAX_CNT;i++) {
		m_servos[i] = NULL;
		m_initAngles[i] = 0;
	}

	m_offsetLocalSupport = false;
	m_offsetMin = 0;
	m_offsetMax = 0;
	memset(m_offsets, 0x00, sizeof(m_offsets));
}

CServoManager::~CServoManager()
{
}

ServoRet_t CServoManager::Init(ServoType_t type, int servoCnt, int *idsOrPins, uint16_t *initAngles,
	int16_t offsetMin, int16_t offsetMax, HardwareSerial *serl, int waitMsec)
{
	ServoRet_t ret = SERVO_RET_SUCCESS;

	do {
		if (servoCnt > SERVO_MAX_CNT) {
			ret = SERVO_RET_LIMITED;
			break;
		}

		switch(type)
		{
		case SERVO_UBT:
		{
			for (int i = 0;i < servoCnt;i++) {
				CServoUBT *ubt = new CServoUBT;
				if (NULL == ubt) {
					ret = SERVO_RET_ERROR;
					break;
				}

				ret = ubt->Init(idsOrPins[i], serl, waitMsec);
				if (ret != SERVO_RET_SUCCESS) {
					break;
				}

				m_servos[i] = ubt;
				m_initAngles[i] = initAngles[i];
			}
		}
			break;
		case SERVO_SG90:
		{
			for (int i = 0;i < servoCnt;i++) {
				CServoSG90 *sg = new CServoSG90;
				if (NULL == sg) {
					ret = SERVO_RET_ERROR;
					break;
				}

				ret = sg->Init(idsOrPins[i]);
				if (ret != SERVO_RET_SUCCESS) {
					break;
				}

				m_servos[i] = sg;
				m_initAngles[i] = initAngles[i];
			}
		}
			break;
		default:
		{
			ret = SERVO_RET_UNSUPPORT;
		}
			break;
		}
		if (SERVO_RET_SUCCESS != ret) {
			break;
		}

		m_servoCnt = servoCnt;
		m_offsetMin = offsetMin;
		m_offsetMax = offsetMax;
	} while(false);

	if (SERVO_RET_SUCCESS != ret) {
		Uninit();
	}

	return ret;
}

void CServoManager::Uninit()
{
	m_servoCnt = 0;
	for (int i = 0;i < SERVO_MAX_CNT;i++) {
		if (NULL != m_servos[i]) {
			m_servos[i]->Uninit();
			delete m_servos[i];
			m_servos[i] = NULL;
		}
		m_initAngles[i] = 0;
	}

	m_offsetMin = 0;
	m_offsetMax = 0;
}

int CServoManager::GetServoCount()
{
	return m_servoCnt;
}

void CServoManager::SetOffsetLocalSupport(bool support)
{
	m_offsetLocalSupport = support;
	if (support) {
		loadOffsets();
	}
}

void CServoManager::SetToInitAngles()
{
	for (int i = 0;i < m_servoCnt;i++) {
		if (NULL != m_servos[i]) {
			m_servos[i]->SetAngle(m_initAngles[i], 1000);
		}
	}
}

uint16_t CServoManager::GetInitAngle(int idx)
{
	if (idx < 0 || idx >= m_servoCnt || NULL == m_servos[idx]) {
		return 0;
	}

	return m_initAngles[idx];
}

ServoRet_t CServoManager::SetAngle(int idx, uint16_t angle, uint32_t msec)
{
	if (idx < 0 || idx >= m_servoCnt || NULL == m_servos[idx]) {
		return SERVO_RET_ERROR;
	}

	return m_servos[idx]->SetAngle(angle, msec);
}

ServoRet_t CServoManager::GetAngle(int idx, uint16_t &angle)
{
	if (idx < 0 || idx >= m_servoCnt || NULL == m_servos[idx]) {
		return SERVO_RET_ERROR;
	}

	return m_servos[idx]->GetAngle(angle);
}

ServoRet_t CServoManager::SetOffset(int idx, int16_t offset)
{
	if (idx < 0 || idx >= m_servoCnt || NULL == m_servos[idx]) {
		return SERVO_RET_ERROR;
	}

	if (m_offsetLocalSupport) {
		return setOffset(idx, offset);
	}
	else {
		return m_servos[idx]->SetOffset(offset);
	}
}

ServoRet_t CServoManager::GetOffset(int idx, int16_t &offset)
{
	if (idx < 0 || idx >= m_servoCnt || NULL == m_servos[idx]) {
		return SERVO_RET_ERROR;
	}

	if (m_offsetLocalSupport) {
		return getOffset(idx, offset);
	}
	else {
		return m_servos[idx]->GetOffset(offset);
	}
}

ServoRet_t CServoManager::SetId(int idx, uint8_t newid)
{
	if (idx < 0 || idx >= m_servoCnt || NULL == m_servos[idx]) {
		return SERVO_RET_ERROR;
	}

	return m_servos[idx]->SetId(newid);
}

ServoRet_t CServoManager::SetLed(int idx, bool on)
{
	if (idx < 0 || idx >= m_servoCnt || NULL == m_servos[idx]) {
		return SERVO_RET_ERROR;
	}

	return m_servos[idx]->SetLed(on);
}

ServoRet_t CServoManager::Terminate(int idx)
{
	if (idx < 0 || idx >= m_servoCnt || NULL == m_servos[idx]) {
		return SERVO_RET_ERROR;
	}

	return m_servos[idx]->Terminate();
}

ServoRet_t CServoManager::GetFirmware(int idx, uint32_t &firmware)
{
	if (idx < 0 || idx >= m_servoCnt || NULL == m_servos[idx]) {
		return SERVO_RET_ERROR;
	}

	return m_servos[idx]->GetFirmware(firmware);
}

ServoRet_t CServoManager::Lock(int idx)
{
	if (idx < 0 || idx >= m_servoCnt || NULL == m_servos[idx]) {
		return SERVO_RET_ERROR;
	}

	return m_servos[idx]->Lock();
}

ServoRet_t CServoManager::Unlock(int idx)
{
	if (idx < 0 || idx >= m_servoCnt || NULL == m_servos[idx]) {
		return SERVO_RET_ERROR;
	}

	return m_servos[idx]->Unlock();
}

void CServoManager::resetOffsets()
{
	memset(m_offsets, 0x00, sizeof(m_offsets));
	saveOffsets();
}

void CServoManager::loadOffsets()
{
	// If some offsets are invalid, resets them to zero
	bool needRst = false;

	EEPROM.begin(1024);

	int addr = EEPROM_SERVO_OFFSET;
	for (int i = 0;i < SERVO_MAX_CNT;i++) {
    EEPROM.get(addr, m_offsets[i]);
    addr += sizeof(m_offsets[i]);

		if (m_offsets[i] < m_offsetMin || m_offsets[i] > m_offsetMax) {
			m_offsets[i] = 0;
			needRst = true;
		}
	}

	if (needRst) {
		saveOffsets();
	}
}

void CServoManager::saveOffsets()
{
	EEPROM.begin(1024);

	int addr = EEPROM_SERVO_OFFSET;
  for (int i = 0;i < SERVO_MAX_CNT;i++) {
    EEPROM.put(addr, m_offsets[i]);
    addr += sizeof(m_offsets[i]);
  }

	EEPROM.commit();
}

ServoRet_t CServoManager::getOffset(int idx, int16_t &offset)
{
	offset = m_offsets[idx];
	return SERVO_RET_SUCCESS;
}

ServoRet_t CServoManager::setOffset(int idx, int16_t offset)
{
	if (offset < m_offsetMin || offset > m_offsetMax) {
		return SERVO_RET_LIMITED;
	}

	m_offsets[idx] = offset;
	saveOffsets();
	return SERVO_RET_SUCCESS;
}

